#!/usr/bin/env python3
import rospy
import threading
from std_msgs.msg import Float64, Int32

class ArmDemo:
    def __init__(self):
        rospy.init_node('arm_demo', anonymous=True)
        rospy.on_shutdown(self.cleanup)
        
        # 关节控制器
        self.joints = [
            rospy.Publisher('/tilt_controller/command', Float64, queue_size=10),
            rospy.Publisher('/shoulder_controller/command', Float64, queue_size=10),
            rospy.Publisher('/elbow_controller/command', Float64, queue_size=10),
            rospy.Publisher('/wrist_controller/command', Float64, queue_size=10),
            rospy.Publisher('/hand_controller/command', Float64, queue_size=10)
        ]
        
        # 初始位置
        self.home_position = [1.0, 3.0, 3.0, 3.0, 3.0]
        
        # 状态管理
        self.is_executing = False
        self.current_sequence_thread = None
        self.stop_requested = False
        
        # 订阅语音指令
        rospy.Subscriber("/voice_controller/commands", Int32, self.voice_command_callback)
        rospy.loginfo("机械臂演示节点就绪，等待指令...")
        
        # 初始位置
        self.move_to_position(self.home_position, "初始位置")
    
    def voice_command_callback(self, msg):
        command = msg.data
        rospy.loginfo(f"收到指令: {command}")
        
        if command == 2:  # 抓取指令
            if self.is_executing:
                rospy.logwarn("机械臂正在执行动作，请稍候")
                return
                
            rospy.loginfo("开始执行抓取序列...")
            self.execute_grasp_sequence()
        elif command == 5:  # 释放指令
            if self.is_executing:
                rospy.logwarn("机械臂正在执行动作，请稍候")
                return
                
            rospy.loginfo("开始执行释放序列...")
            self.execute_grasp_sequence2()    
        elif command == 0:  # 停止指令
            rospy.loginfo("收到停止指令")
            self.stop_requested = True
    
    def execute_grasp_sequence(self):
        """在新线程中执行抓取序列"""
        if self.current_sequence_thread and self.current_sequence_thread.is_alive():
            rospy.logwarn("已有序列正在执行")
            return
            
        self.is_executing = True
        self.stop_requested = False
        self.current_sequence_thread = threading.Thread(
            target=self._grasp_sequence_worker, 
            daemon=True
        )
        self.current_sequence_thread.start()
    def execute_grasp_sequence2(self):
        """在新线程中执行抓取序列"""
        if self.current_sequence_thread and self.current_sequence_thread.is_alive():
            rospy.logwarn("已有序列正在执行")
            return
            
        self.is_executing = True
        self.stop_requested = False
        self.current_sequence_thread = threading.Thread(
            target=self._grasp_sequence_worker2, 
            daemon=True
        )
        self.current_sequence_thread.start()    
    def _grasp_sequence_worker(self):
        """抓取序列的工作线程"""
        try:
            # 1. 移动到初始位置
            self.move_to_position([1.0, 3.0, 3.0, 3.0, 3.0], "初始位置")
            if self.stop_requested: return
            rospy.sleep(5)
            
            # 2. 移动到抓取位置
            self.move_to_position([1.0, 3.4, 3.2, 3.0, 3.0], "抓取位置")
            if self.stop_requested: return
            rospy.sleep(5)
            
            # 3. 执行抓取动作
            self.move_to_position([1.0, 3.4, 3.2, 3.0, 2.0], "抓取中")
            if self.stop_requested: return
            rospy.sleep(5)
            
            # 4. 移动到释放位置
            self.move_to_position([1.0, 3.0, 3.0, 3.0, 2.0], "释放位置")
            if self.stop_requested: return
            rospy.sleep(5)
            
            
            
            rospy.loginfo("抓取序列完成")
            
        except Exception as e:
            rospy.logerr(f"抓取序列出错: {str(e)}")
        finally:
            self.is_executing = False
            self.stop_requested = False
    def _grasp_sequence_worker2(self):
        """抓取序列的工作线程"""
        try:
            # 1. 移动到初始位置
            self.move_to_position([1.0, 3.0, 3.0, 3.0, 2.0], "初始位置")
            if self.stop_requested: return
            rospy.sleep(5)
            
            
            # 5. 释放物体
            self.move_to_position([1.0, 3.0, 3.0, 3.0, 3.0], "释放完成")
            if self.stop_requested: return
            rospy.sleep(5)
            
            rospy.loginfo("抓取序列完成")
            
        except Exception as e:
            rospy.logerr(f"抓取序列出错: {str(e)}")
        finally:
            self.is_executing = False
            self.stop_requested = False
    
    def move_to_position(self, positions, description):
        """直接移动到指定位置（无平滑处理）"""
        if self.stop_requested:
            rospy.loginfo("移动被停止")
            return
            
        rospy.loginfo(f"移动到: {description}")
        
        # 发布所有关节位置
        for pub, pos in zip(self.joints, positions):
            msg = Float64()
            msg.data = pos
            pub.publish(msg)
    
    def cleanup(self):
        """关闭时的清理工作"""
        rospy.loginfo("关闭机械臂演示节点...")
        self.stop_requested = True
        if self.current_sequence_thread and self.current_sequence_thread.is_alive():
            rospy.loginfo("等待动作序列完成...")
            self.current_sequence_thread.join(timeout=2.0)

if __name__ == '__main__':
    try:
        controller = ArmDemo()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass
